"""
  此文件从过滤的数据中提取出自由驾驶的数据
"""

# !处理路径导入问题（添加绝对路径）！！！
import sys
import os
CODE_INTERNAL_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) # 生成Code文件夹内部对应的绝对路径
sys.path.append(CODE_INTERNAL_PATH)

# 导入内部包
from utils.read_data import read_original_data
from utils.write_data import write_data_to_file

# 文件路径常量
FILE_PATH_I80_1_from = "../../Data/Ngsim数据集/I80数据集/2. 过滤数据/trajectories-0400-0415_filter.txt" # 2052 884 18
FILE_PATH_I80_2_from = "../../Data/Ngsim数据集/I80数据集/2. 过滤数据/trajectories-0500-0515_filter.txt" # 1836 526 18
FILE_PATH_I80_3_from = "../../Data/Ngsim数据集/I80数据集/2. 过滤数据/trajectories-0515-0530_filter.txt" # 1790 1153 18
FILE_PATH_101_1_from = "../../Data/Ngsim数据集/101数据集/2. 过滤数据/trajectories-0750am-0805am_filter.txt" # 2169 437 18
FILE_PATH_101_2_from = "../../Data/Ngsim数据集/101数据集/2. 过滤数据/trajectories-0805am-0820am_filter.txt" # 2017 569 18
FILE_PATH_101_3_from = "../../Data/Ngsim数据集/101数据集/2. 过滤数据/trajectories-0820am-0835am_filter.txt" # 1915 421 18

FILE_PATH_I80_1_to = "../../Data/Ngsim数据集/I80数据集/3. 提取数据/3. 自由驾驶数据/trajectories-0400-0415_free_drive.txt" # 2896 150 7
FILE_PATH_I80_2_to = "../../Data/Ngsim数据集/I80数据集/3. 提取数据/3. 自由驾驶数据/trajectories-0500-0515_free_drive.txt" # 2303 150 7
# FILE_PATH_I80_3_to = "../../Data/Ngsim数据集/I80数据集/3. 提取数据/3. 自由驾驶数据/trajectories-0515-0530_free_drive.txt" # 
# FILE_PATH_101_1_to = "../../Data/Ngsim数据集/101数据集/3. 提取数据/3. 自由驾驶数据/trajectories-0750am-0805am_free_drive.txt" # 
# FILE_PATH_101_2_to = "../../Data/Ngsim数据集/101数据集/3. 提取数据/3. 自由驾驶数据/trajectories-0805am-0820am_free_drive.txt" # 
# FILE_PATH_101_3_to = "../../Data/Ngsim数据集/101数据集/3. 提取数据/3. 自由驾驶数据/trajectories-0820am-0835am_free_drive.txt" # 

# 其他常量
TARGET_FRAMES = 150 # 目标帧数（要收集的数据数量）

def is_valid_lane(cur_lane):
  return cur_lane >= 1 and cur_lane <= 5

def is_free_driving(cur_front_carId):
  return cur_front_carId == 0

def collect_free_drive_data(filter_data, start_idx):
  free_driving_data = []
  for i in range(TARGET_FRAMES):
    self_car_item = filter_data[start_idx + i]
    free_driving_data.append([
      self_car_item[1], # 数据帧
      self_car_item[0], # 编号
      self_car_item[4], # x坐标（横轴）
      self_car_item[5], # y坐标（纵轴）
      self_car_item[11], # 速度
      self_car_item[12], # 加速度
      self_car_item[13] # 车道编号
    ])
  return free_driving_data

def extract_free_drive_data(filter_datas, lane_idx=13, front_carId_idx=14):
  # 收集到的若干自由驾驶数据组
  free_drive_datas = []

  # 当前稳定达到的自由驾驶帧数
  accumulated_follow_frams = 0

  for i in range(len(filter_datas)):
    for j in range(len(filter_datas[i])):
      cur_laneId = filter_datas[i][j][lane_idx]
      cur_front_carId = filter_datas[i][j][front_carId_idx]

      if not is_free_driving(cur_front_carId) or not is_valid_lane(cur_laneId):
        # 前车编号不为0，不是自由驾驶
        accumulated_follow_frams = 0
        continue
      # 只要没有前车编号，就是自由驾驶
      else:
        accumulated_follow_frams += 1
        if accumulated_follow_frams >= TARGET_FRAMES:
          # 收集自由驾驶数据
          free_drive_data = collect_free_drive_data(filter_datas[i], j - TARGET_FRAMES + 1)
          free_drive_datas.append(free_drive_data)

  return free_drive_datas

def filter_error_group(free_drive_datas, self_car_v_idx=4):
  filtered_datas = []

  has_error_data = False

  for i, follow_data in enumerate(free_drive_datas):
    # 检测这组数据是否有问题
    for j, data in enumerate(follow_data):
      if not is_valid_free_drive_data(data, self_car_v_idx):
        print("出现一条不符合规则的跟车数据", i, j)
        has_error_data = True
        break
    
    if not has_error_data:
      # 收集这组数据
      filtered_datas.append(follow_data)

    has_error_data = False

  return filtered_datas

def is_valid_free_drive_data(data, self_car_v_idx):
  self_car_v = data[self_car_v_idx]

  if self_car_v <= 0:
    print("车速出现负值", self_car_v)
    return False

  return True

def handle(fromFileName, toFileName, index):
  # 读取数据
  filter_datas = read_original_data(fromFileName)

  # 自由驾驶数据
  free_drive_datas = extract_free_drive_data(filter_datas)

  # 过滤出含非法值的数据组
  filtered_datas = filter_error_group(free_drive_datas)

  print("第几组数据", index + 1)
  print("提取结果", len(free_drive_datas), len(free_drive_datas[1]), len(free_drive_datas[0][0]))
  print("过滤结果", len(filtered_datas), len(filtered_datas[0]), len(filtered_datas[0][0]))

  # 写入数据
  write_data_to_file(filtered_datas, toFileName)

if __name__ == "__main__":
  fileNames = [
    [FILE_PATH_I80_1_from, FILE_PATH_I80_1_to],
    [FILE_PATH_I80_2_from, FILE_PATH_I80_2_to],
  ]

  for i in range(len(fileNames)):
    fileName = fileNames[i]
    handle(fileName[0], fileName[1], i)